# -*- encoding: UTF-8 -*-

import math
import thread
import threading
from naoqi import ALModule
from naoqi import ALBroker
from naoqi import ALProxy
import sys
import os
import time
# import vision
# import locate1
# import cv2
import numpy as np

NAO_IP = "192.168.1.107"
NAO_PORT = 9559
myBroker1 = ALBroker("myBroker",
                     "0.0.0.0",  # listen to anyone
                     0,  # find a free port and use it
                     NAO_IP,  # parent broker IP
                     NAO_PORT)
sonarproxy = ALProxy("ALSonar")
sonarproxy.subscribe("myApplication")
memorypxy = ALProxy("ALMemory")
alproxy = ALProxy("ALMotion")
cam = ALProxy("ALVideoDevice")
tts = ALProxy("ALTextToSpeech")
posture = ALProxy("ALRobotPosture")

obstacle_R = 0.24  # 障碍物半径 0.25
# obstacle_w =0.17
car_r = 0.4
length = 0.23  # 机器人半径 0.24
finaldis = 1.0
MINdis = 0.7
leftdis = 8
rightdis = 8
mindis = 0.5
mistake_turnstra = 0.2
mistake_turn1 = 0.2
mistake_turn2 = 0.2
speed_x = 0.07
speed_y = 0.0
speed_z = 0.0
angleleft = 0.0
angleright = 0.0
# finalcoor=(0.0,0.0)
# l=threading.RLock()
flag = 1
sonarflag = 0
initangle = 0.0

class mythread(threading.Thread):
    def __init__(self, name):
        threading.Thread.__init__(self)
        self.name = name

    def run(self):
        while True:
            global speed_x, leftdis, rightdis, fixangle, flag, sonarflag
            leftdis, rightdis = sonardetect()
            print "%f,%f" % (leftdis, rightdis)
            if (min(leftdis, rightdis) < mindis):
                setRoute(min(leftdis, rightdis) - 0.1)
                sonarproxy.unsubscribe("myApplication")
                break
            time.sleep(0.8)

class runthread(threading.Thread):
    def __init__(self, name):
        threading.Thread.__init__(self)
        self.name = name

    def run(self):
        while True:
            # global speed_x,speed_y,speed_z
            alproxy.move(speed_x, speed_y, speed_z)
            if (flag != 1):
                alproxy.stopMove()
                break
            time.sleep(0.2)

def sonardetect():
    ldis = memorypxy.getData("Device/SubDeviceList/US/Left/Sensor/Value")
    rdis = memorypxy.getData("Device/SubDeviceList/US/Right/Sensor/Value")
    sldis = str(ldis)
    srdis = str(rdis)
    sldis = sldis[0:4]
    srdis = srdis[0:4]

    sldis = float(sldis)
    srdis = float(srdis)

    LEFTDISTANCE = sldis
    RIGHTDISTANCE = srdis
    return (LEFTDISTANCE, RIGHTDISTANCE)

def processdata(data):
    # print initangle
    if (data >= 0 and initangle <= 0):
        if (math.pi + initangle >= data):
            return -(data - initangle)
        else:
            return 2 * math.pi + initangle - data
    elif (data <= 0 and initangle >= 0):
        if (math.pi - initangle >= -data):
            return initangle - data
        else:
            return -(2 * math.pi - initangle + data)
    else:
        return initangle - data

def setRoute(dis):
    r=dis+obstacle_R-math.pow(obstacle_R+length,2)/(dis+obstacle_R)
    turn_degree = math.atan((obstacle_R+length)/r)
    #X = (obstacle_R + length) * math.sin(turn_degree)
    speed = 0.08
    '''
    leftArmEnable = False
    rightArmEnable = False
    alproxy.setMoveArmsEnabled(leftArmEnable, rightArmEnable)
    '''
    global speed_x, speed_y, speed_z, flag, initangle
    '''
     z = -(math.pi-2*math.atan(dis/bottom))
     x = dis / math.sin(-z)
     y=0'''
    t1=r*turn_degree/speed
    speed_x = speed
    fixangle = -processdata(memorypxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value"))
    speed_z = math.fabs(-turn_degree + fixangle) / t1
    speed_y = 0
    time.sleep(t1)
    '''
    speed_y = y/t1
    speed_x=x/t1
    speed_z=z/t1
    '''

    '''
    t2=10#3.5
    speed_y = 0
    if (dis - X > 0.02):
        speed_x=2.3*z*X/math.sin(z)/t2
    else:
        speed_x=2.3*(X+bottom/math.tan(-z)-car_r*math.cos(z))*z/math.sin(z)/t2
    #print X,speed_x
    speed_z=-2*z/t2
    time.sleep(t2)'''
    fixangle = -turn_degree - processdata(memorypxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value"))
    speed_x = speed
    t2 = (obstacle_R+length)*math.fabs(2*turn_degree+fixangle) / speed
    speed_y = 0
    speed_z = (2*fixangle+turn_degree) / t2
    time.sleep(t2)

    '''
    fixangle = -processdata(memorypxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value"))
    #t4=3
    speed_x=speed
    if (math.fabs(fixangle) > 0.08):
        t4 = (finaldis+obstacle_R-X) * fixangle / math.sin(fixangle) / speed_x
    else:
        t23 = (finaldis+obstacle_R-X) / speed_x
    speed_y=0
    speed_z=fixangle/t4
    time.sleep(t4+1)'''
    # flag=0

    '''
    t3=10
    if (dis - X > 0.02):
        speed_x = x / t3
    else:
        speed_x = car_r*(1+math.cos(z))/math.sin(-z)/t3
    speed_z = z / t3
    speed_y = 0.0
    time.sleep(t3)  # 4'''

    speed_x = speed
    fixangle = turn_degree-processdata(memorypxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value"))
    t3 = r * math.fabs(turn_degree+fixangle) / speed
    speed_z = (-turn_degree + fixangle) / t3
    speed_y = 0
    time.sleep(t3)

    flag = 0

# thread
event = threading.Event()
x = mythread("A")
run = runthread("B")
# vision
'''
resolution = 3  # Image of 1280*960px
colorSpace = 13
fps = 30  # Supported Framerate: from 1 to 30 fps
string=time.strftime('%X')
nameId = cam.subscribeCamera(string, 0, resolution, colorSpace, fps)'''
# Global variable to store the ReactToTouch module instance
ReactToTouch = None
memory = None

class ReactToTouch(ALModule):
    """ A simple module able to react
        to touch events.
    """

    def __init__(self, name):
        ALModule.__init__(self, name)
        # No need for IP and port here because
        # we have our Python broker connected to NAOqi broker

        # Create a proxy to ALTextToSpeech for later use
        self.tts = ALProxy("ALTextToSpeech")
        self.move = ALProxy("ALMotion")
        self.tts.say("start")
        # Subscribe to TouchChanged event:
        global memory
        memory = ALProxy("ALMemory")
        memory.subscribeToEvent("TouchChanged",
                                "ReactToTouch",
                                "onTouched")

    def onTouched(self, strVarName, value):
        """ This will be called each time a touch
        is detected.

        """
        # Unsubscribe to the event when talking,
        # to avoid repetitions
        memory.unsubscribeToEvent("TouchChanged",
                                  "ReactToTouch")

        touched_bodies = []
        for p in value:
            if p[1]:
                print p[0]
                touched_bodies.append(p[0])
                if p[0] == "Head/Touch/Middle":
                    #setFinger()
                    pass
                if p[0] == "Head/Touch/Front":
                    _main()
        # self.say(touched_bodies)
        # Subscribe again to the event
        memory.subscribeToEvent("TouchChanged",
                                "ReactToTouch",
                                "onTouched")

def main_():
    global ReactToTouch
    ReactToTouch = ReactToTouch("ReactToTouch")
    try:
        while True:
            time.sleep(1)
    except KeyboardInterrupt:
        print
        print "Interrupted by user, shutting down"
        myBroker1.shutdown()
        sys.exit(0)

def main():
    alproxy.move(0.05, 0.0, 0.0)
    # direction=getdirection()
    # alproxy.moveTo(0.0,0.0,direction)
    # 0.464350288425 0.390809439382
    # 0.464350288425 0.0876114540325

    # 0.464350288425 1.56323775753
    # 0.464350288425 0.35044581613

def _main():
    global rightdis, leftdis, flag, initangle
    # pic = vision.getimg(NAO_IP, NAO_PORT)
    # ge = threading.Thread(target=geterror, args=(pic,))
    # ge.start()
    # time.sleep(1.0)
    initangle = memorypxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")
    x.start()
    run.start()
    '''
    while True:
        #print ("??%s" % leftdis)
        #print ("??%s" % rightdis)
        if (min(rightdis, leftdis) <= mindis):
            #tts.say("检测到左方障碍物")
            print "@#"
            #setRoute(min(rightdis, leftdis))
            break
        #time.sleep(0.2)
    #flag = 0
    '''

if __name__ == "__main__":
    # main()
    # geterror(vision.getimg(NAO_IP, NAO_PORT))
    # run.start()
    # x.start()
    _main()
    #main_()
    # setRoute(0.66)

